// -*- c -*-
//1/24/05 testmotors function is fixed. dpm
//1/2/06 added camera test. aw
//1/5/06 added sonar test. dpm
//1/5/06 added additional servo funtionality TG
//1/5/06 added track_angle functionality to vision test. JP

int aaa;
float bbb;
int aaaar[10];

#use "gbamenu.ic"
#use "xbccamlib.ic"

void main()
{
    char a[5][16]={"Motors", "Servos", "Sonars", "Vision", "Camera"};
    int sel=0;
    
    while(1) {
        sel = select_string(a, sel);
        if(sel==-1) {
            printf("Exit\n");
            break;
        }
        else if(sel == 0) testmotors();
        else if(sel == 1) testservo();
        else if(sel == 2) testsonar();
        else if(sel == 3) testvision();
        else if(sel == 4) testcamera();
    }
}

void

testmotors(void)
{
    printf("Motor test, Hold B to end    \n");
    while (!escape_button()) {
        int i;
        for (i= 0; (!escape_button() && (i< 2)); i++) {
            motor(i,70); msleep(250L);
            off(i);
            motor(i+2,70); msleep(250L);
            off(i+2);
        }
        for (i= 0; (!escape_button() && (i< 2)); i++) {
            motor(i,-70); msleep(250L);
            off(i);
            motor(i+2,-70); msleep(250L);
            off(i+2);
        }
    }
    beep();
}

void testservo(void)
{
    int i, s;
    int x, y;
    
    while(a_button());
    printf("Attach Servos, Press A\n");
    while(!a_button());
    
    printf("Press B to stop\n Servo=");
    display_get_xy(&x, &y);
    
    enable_servos();
    while(!b_button()) {
        for (i=20;(!b_button() && i<205);i+=5) {
            display_set_xy(x, y);
            printf("    ");
            display_set_xy(x, y);
            printf("%d", i);
            
            for(s=0; s<4; s++) {
                set_servo_position(s, i);
                msleep(40L);
            }
        }
    }
    while(!a_button()){
        display_set_xy(1, y);
        printf("servo=%d, \n  use Right-Left PAD\n  press A to end",i);
        if(left_button()&& i>=5)i-=5;
        if(right_button() && i<=250)i+=5;
        for(s=0; s<4; s++) {
            set_servo_position(s, i);
        }
        msleep(40L);
        
    }
    disable_servos();
    
    beep();
}

void testsonar(void)
{
    int i, s;
    
    while(a_button());
    printf("Attach Sonar(s), Press A\n");
    while(!a_button());
    display_clear();
    printf("Press B to end\n");
    while(!b_button()) {
        for (i=8;(!b_button() && i<=15);i++) {
            display_clear_at(0,i-5,20);
            printf("Sonar %d: %d", i, sonar(i));
            msleep(40L);
        }
    }
    beep();
}

// Helper functions for display of vision stats
#define ROW_WIDTH 28
#define DATA_COL 10
#define DATA_WIDTH (ROW_WIDTH-DATA_COL)

#define CH_ROW 1
#define FNUM_ROW 2
#define N_ROW 3

#define AREA_ROW 6
#define BBOX_ROW 7
#define X_ROW 8
#define Y_ROW 9
#define CONF_ROW 10
#define STATUS_ROW 11

void print_string_at(int x, int y, char s[])
{
    display_clear_at(x, y, ROW_WIDTH-x);
    display_set_xy(x, y);
    printf("%s", s);
}
void print_int_at(int x, int y, int val)
{
    display_clear_at(x, y, ROW_WIDTH-x);
    display_set_xy(x, y);
    printf("%d", val);
}
void print_long_at(int x, int y, long val)
{
    display_clear_at(x, y, ROW_WIDTH-x);
    display_set_xy(x, y);
    printf("%l     ", val);
}
void print_float_at(int x, int y, float val)
{
    display_clear_at(x, y, ROW_WIDTH-x);
    display_set_xy(x, y);
    printf("%f     ", val);
}
void print_bbox_stats_at(int x, int y, int ch, int i)
{
    display_clear_at(x, y, ROW_WIDTH-x);
    display_set_xy(x, y);
    printf("%d w x %d h", 
           track_bbox_width(ch, i), 
           track_bbox_height(ch, i));
}
void print_x_stats_at(int x, int y, int ch, int i)
{
    display_clear_at(x, y, ROW_WIDTH-x);
    display_set_xy(x, y);
    printf("%d\t(%d -> %d)", 
           track_x(ch, i),
           track_bbox_left(ch, i), 
           track_bbox_right(ch, i));
}
void print_y_stats_at(int x, int y, int ch, int i)
{
    display_clear_at(x, y, ROW_WIDTH-x);
    display_set_xy(x, y);
    printf("%d\t(%d -> %d)", 
           track_y(ch, i),
           track_bbox_top(ch, i), 
           track_bbox_bottom(ch, i));
}
void testvision()
{
    int ch = 0;
    int cur_state = 0; // cur_state is 0 for testvision1 and 1 for testvision2
    track_enable_orientation();
    while (ch > -1) {
        if (cur_state == 0) { 
            ch = testvision1(ch);
        }
        else  {
            ch = testvision2(ch);
        }
        cur_state = (cur_state+1)%2;
    }
    
}
int testvision1(int ich)
{
    int ch=0, n;
    long fnum;
    int p_bstate, bstate;
    ch = ich;   
    display_clear();
    printf("Vision Test BBOX \n");
    print_string_at(0, CH_ROW, "  Channel: \n");
    print_string_at(0, FNUM_ROW, "  Frame:   \n");
    print_string_at(0, N_ROW, "  Count:   \n\n");
    
    printf("Largest Blob Stats\n");
    print_string_at(0, AREA_ROW, "  Area:\n");
    print_string_at(0, BBOX_ROW, "  Bbox:\n");
    print_string_at(0, X_ROW, "  X:\n");
    print_string_at(0, Y_ROW, "  Y:\n");
    print_string_at(0, CONF_ROW, "  Conf:\n");
    
    printf("L button for Orientation\n");
    printf("A: Show blobs, Down: Exit\n");
    printf("Left/Right: Change channel");
    
    /* Initialize the button state */
    p_bstate = bstate = button_state();
    
    while(!down_button()) {
        // Check for buttons
        p_bstate = bstate;
        bstate = button_state();
        
        if(button_hit(L_BTN, p_bstate, bstate) ) {
            return ch;
        }
        else if(button_hit(LEFT_BTN, p_bstate, bstate) && ch > 0) {
            ch--;
        }
        else if(button_hit(RIGHT_BTN, p_bstate, bstate) && ch < 2) {
            ch++;
        }
        else if(button_hit(A_BTN, p_bstate, bstate)) {
            track_show_display(1, 1, 1 << ch);
        }
        
        // Check if new data is available.  
        if(track_is_new_data_available()) {
            // New data is available, update to use it
            track_update();
            
            // Get the frame number
            fnum = track_get_frame();
            
            // Get the number of blobs found in this frame
            n=track_count(ch);
            
            // Print channel, frame, count
            print_int_at(DATA_COL, CH_ROW, ch);
            print_long_at(DATA_COL, FNUM_ROW, fnum);
            print_int_at(DATA_COL, N_ROW, n);
            
            if(n>0) {
                // There's at least one blob, show stats
                print_int_at(DATA_COL, AREA_ROW, track_size(ch, 0));
                print_bbox_stats_at(DATA_COL, BBOX_ROW, ch, 0);
                print_x_stats_at(DATA_COL, X_ROW, ch, 0);
                print_y_stats_at(DATA_COL, Y_ROW, ch, 0);
                print_int_at(DATA_COL, CONF_ROW, track_confidence(ch, 0));
            }
            else {
                // No blobs, just clear out fields
                display_clear_at(DATA_COL, AREA_ROW, DATA_WIDTH);
                display_clear_at(DATA_COL, BBOX_ROW, DATA_WIDTH);
                display_clear_at(DATA_COL, X_ROW, DATA_WIDTH);
                display_clear_at(DATA_COL, Y_ROW, DATA_WIDTH);
                display_clear_at(DATA_COL, CONF_ROW, DATA_WIDTH);
            }
        }
        else {
            // No new data yet, print a dot, return to the top of the
            // loop and check again
            printf(".");
        }
        
    }
    beep();
    return -2;
}
int testvision2(int ich)
{
    int ch=0, n;
    long fnum;
    int p_bstate, bstate;
    ch = ich;   
    display_clear();
    printf("Vision Test Orientation\n");
    print_string_at(0, CH_ROW, "  Channel: \n");
    print_string_at(0, FNUM_ROW, "  Frame:   \n");
    print_string_at(0, N_ROW, "  Count:   \n\n");
    
    printf("Largest Blob Stats\n");
    print_string_at(0, AREA_ROW, "  Areas:\n");
    print_string_at(0, BBOX_ROW, "  Major:\n");
    print_string_at(0, X_ROW, "  Minor:\n");
    print_string_at(0, Y_ROW, "  Angle:\n");
    printf("R button for BBOX stats\n");
    printf("A: Show blobs, Down: Exit\n");
    printf("Left/Right: Change channel");
    
    /* Initialize the button state */
    p_bstate = bstate = button_state();
    
    while(!down_button()) {
        // Check for buttons
        p_bstate = bstate;
        bstate = button_state();
        if(button_hit(R_BTN, p_bstate, bstate) ) {
            return ch;
        }
        else  if(button_hit(LEFT_BTN, p_bstate, bstate) && ch > 0) {
            ch--;
        }
        else if(button_hit(RIGHT_BTN, p_bstate, bstate) && ch < 2) {
            ch++;
        }
        else if(button_hit(A_BTN, p_bstate, bstate)) {
            track_show_display(1, 1, 1 << ch);
        }
        
        // Check if new data is available.  
        if(track_is_new_data_available()) {
            // New data is available, update to use it
            track_update();
            
            // Get the frame number
            fnum = track_get_frame();
            
            // Get the number of blobs found in this frame
            n=track_count(ch);
            
            // Print channel, frame, count
            print_int_at(DATA_COL, CH_ROW, ch);
            print_long_at(DATA_COL, FNUM_ROW, fnum);
            print_int_at(DATA_COL, N_ROW, n);
            
            if(n>0) {
                // There's at least one blob, show stats
                print_int_at(DATA_COL, AREA_ROW, track_size(ch, 0));
                print_float_at(DATA_COL, BBOX_ROW, track_major_axis(ch, 0));
                print_float_at(DATA_COL, X_ROW, track_minor_axis(ch, 0));
                print_float_at(DATA_COL, Y_ROW, track_angle(ch, 0));
            }
            else {
                // No blobs, just clear out fields
                display_clear_at(DATA_COL, AREA_ROW, DATA_WIDTH);
                display_clear_at(DATA_COL, BBOX_ROW, DATA_WIDTH);
                display_clear_at(DATA_COL, X_ROW, DATA_WIDTH);
            }
        }
        else {
            // No new data yet, print a dot, return to the top of the
            // loop and check again
            printf(".");
        }
        
    }
    beep();
    return -2;
}

#define CAM_AWB_ROW 1
#define CAM_RED_ROW 2
#define CAM_BLUE_ROW 3

#define CAM_AEC_ROW 5
#define CAM_EXP_ROW 6

#define CAM_AGC_ROW 8
#define CAM_GAIN_ROW 9

#define CAM_MIN_ROW 1
#define CAM_MAX_ROW 9

int cam_get_val(int row) 
{
    if(row == CAM_AWB_ROW) {
        return(camera_get_awb());
    }
    else if(row == CAM_RED_ROW) {
        return(camera_get_wb_red_temp());
    }
    else if(row == CAM_BLUE_ROW) {
        return(camera_get_wb_blue_temp());
    }
    else if(row == CAM_AEC_ROW) {
        return(camera_get_aec());
    }
    else if(row == CAM_EXP_ROW) {
        return(camera_get_exposure());
    }
    else if(row == CAM_AGC_ROW) {
        return(camera_get_agc());
    }
    else if(row == CAM_GAIN_ROW) {
        return(camera_get_gain());
    }
}

int cam_set_val(int row, int val) 
{
    if(row == CAM_AWB_ROW) {
        return(camera_set_awb(val));
    }
    else if(row == CAM_RED_ROW || row == CAM_BLUE_ROW) {
        int color_temp[2]; 
        camera_get_wb_color_temp(color_temp); 
        color_temp[row-CAM_RED_ROW] = val;
        return(camera_set_wb_color_temp(color_temp));
    }
    else if(row == CAM_AEC_ROW) {
        return(camera_set_aec(val));
    }
    else if(row == CAM_AGC_ROW) {
        return(camera_set_agc(val));
    }
    else if(row == CAM_GAIN_ROW) {
        return(camera_set_gain(val));
    }
}
void cam_change_val(int row, int amt) 
{
    int oldval=cam_get_val(row);
    int newval=oldval;
    
    // Check for non-standard skip amounts
    if(row == CAM_GAIN_ROW) {
        // Gain is a 6-bit value, skip 4 at a time
        amt *= 4;
    }
    
    // Check if it's a boolean value
    if(row == CAM_AWB_ROW || 
       row == CAM_AEC_ROW || 
       row == CAM_AGC_ROW) {
        if(amt<0) newval=0;
        if(amt>0) newval=1;
    }
    else {
        newval = oldval+amt;
        if(newval<0) newval=0;
        if(newval>255) newval=255;
    }
    cam_set_val(row, newval);
}

void 
testcamera(void)
{
    int row=CAM_AWB_ROW, n;
    long fnum;
    int p_bstate, bstate;
    
    display_clear();
    printf("Camera Test\n");
    print_string_at(0, CAM_AWB_ROW, "* AWB: \n");
    print_string_at(0, CAM_RED_ROW, "   Red: \n");
    print_string_at(0, CAM_BLUE_ROW, "   Blue: \n");
    
    print_string_at(0, CAM_AEC_ROW, "  AEC: \n");
    print_string_at(0, CAM_EXP_ROW, "   Exp: \n");
    
    print_string_at(0, CAM_AGC_ROW, "  AGC: \n");
    print_string_at(0, CAM_GAIN_ROW, "   Gain: \n\n");
    
    printf("UP/Down: Select Param\n");
    printf("Left/Right: Change value\n");
    printf("A: Show video, L/R: Exit");
    
    /* Initialize the button state */
    p_bstate = bstate = button_state();
    
    while(!l_button() && !r_button()) {
        {
            int oldrow = row;
            int color_temp[2]; 
            camera_get_wb_color_temp(color_temp); 
            
            // Check for buttons
            p_bstate = bstate;
            bstate = button_state();
            if(button_hit(LEFT_BTN, p_bstate, bstate)) { 
                cam_change_val(row, -1);
            }
            else if(button_hit(RIGHT_BTN, p_bstate, bstate)) {
                cam_change_val(row, 1);
            }
            else if(button_hit(UP_BTN, p_bstate, bstate)) { 
                if(row>CAM_MIN_ROW) row--;
                if(row == CAM_BLUE_ROW+1 || row == CAM_EXP_ROW+1) {
                    // Empty row, skip it
                    row--;
                }
                if(row == CAM_EXP_ROW) {
                    // Read only row, skip it
                    row--;
                }
            }
            else if(button_hit(DOWN_BTN, p_bstate, bstate)) {
                if(row<CAM_MAX_ROW) row++;
                if(row == CAM_EXP_ROW) {
                    // Read only row, skip it
                    row++;
                }
                if(row == CAM_AEC_ROW-1 || row == CAM_AGC_ROW-1) {
                    // Empty row, skip it
                    row++;
                }
            }
            else if(button_hit(A_BTN, p_bstate, bstate)) {
                track_show_display(1, 1, 1);
            }
            
            if(oldrow != row) {
                // Clear * from old row, put in new row
                display_clear_at(0, oldrow, 1);
                display_set_xy(0, row);
                printf("*");
            }
            
            // Print camera settings
            print_int_at(DATA_COL, CAM_AWB_ROW, camera_get_awb());
            print_int_at(DATA_COL+1, CAM_RED_ROW, color_temp[0]);
            print_int_at(DATA_COL+1, CAM_BLUE_ROW, color_temp[1]);
            
            print_int_at(DATA_COL, CAM_AEC_ROW, camera_get_aec());
            print_int_at(DATA_COL+1, CAM_EXP_ROW, camera_get_exposure());
            
            print_int_at(DATA_COL, CAM_AGC_ROW, camera_get_agc());
            print_int_at(DATA_COL+1, CAM_GAIN_ROW, camera_get_gain());
        }
    }
    beep();
}

// For emacs to interpret formatting uniformly despite dotfile differences:
//   Local variables:
//    comment-column: 40
//    c-indent-level: 4
//    c-basic-offset: 4
//   End:
